#include "nav_rvo.h"
#include <utility>
#include <iostream>
#include "rvo_ros/UAVPosVel.h"

namespace RVO
{

    // 使用了成员初始化列表和动态内存分配。
    RVOPlanner::RVOPlanner(std::string simulation) : simulator(std::move(simulation))
    {
        sim = new RVO::RVOSimulator();
    }

    // 用于设置智能体仿真场景的默认参数。
    void RVOPlanner::setupScenario(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed)
    {
        sim->setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, radius, maxSpeed, Vector3(0.0f, 0.0f, 0.0f)); // 修改这里，添加 Vector3
    }

    // 用于为仿真中的每个智能体设置目标位置。当智能体接近其目标位置时，该函数会切换其目标位置。
    void RVOPlanner::setGoal()
    {
        for (size_t i = 0; i < sim->getNumAgents(); ++i)
        {
            // 如果智能体i与其目标之间的平方距离小于阈值，则切换目标位置，将目标位置切换到反方向，可以实现智能体在两个位置之间来回移动的效果。
            if (absSq(goals[i] - sim->getAgentPosition(i)) < goal_threshold)
            {
                goals[i] = -goals[i]; // 将目标位置设置为其反方向
            }
        }
    }

    // 用于设置仿真中每个智能体的目标位置。
    void RVOPlanner::setGoal(std::vector<geometry_msgs::Point> set_goals)
    {
        goals.clear();                       // 清空当前的目标列表
        int num_agent = sim->agents_.size(); // 获取智能体的数量

        // 检查目标数量是否小于智能体数量
        if (set_goals.size() < num_agent)
        {
            std::cout<<num_agent<<std::endl;
            std::cout<<set_goals.size()<<std::endl;
            std::cout << "error: The num of goals is less than agents" << std::endl; // 输出错误信息
        }
        else
        {
            // 遍历每个智能体并设置目标
            for (int i = 0; i < num_agent; i++)
            {
                float x = set_goals[i].x; // 获取目标的x坐标
                float y = set_goals[i].y; // 获取目标的y坐标
                float z = set_goals[i].z; // 获取目标的z坐标

                goals.emplace_back(Vector3(x, y, z));                                                                // 将目标位置添加到目标列表中
                std::cout << "goal" + std::to_string(i + 1) + ":[" << x << "," << y << "," << z << "]" << std::endl; // 输出目标信息
            }
        }
    }

    // 用于为每个智能体随机生成一个新的目标位置，目标位置在指定的范围内。
    void RVOPlanner::randomOnceGoal(const float limit_goal[6])
    {
        // 提取目标位置的范围
        float x_min = limit_goal[0];
        float x_max = limit_goal[1];
        float y_min = limit_goal[2];
        float y_max = limit_goal[3];
        float z_min = limit_goal[4];
        float z_max = limit_goal[5];

        // 初始化随机数生成器
        std::random_device rd;
        std::default_random_engine e(rd());
        std::uniform_real_distribution<float> ux(x_min, x_max);
        std::uniform_real_distribution<float> uy(y_min, y_max);
        std::uniform_real_distribution<float> uz(z_min, z_max);
        std::uniform_int_distribution<int> ur(0, 10);

        // 遍历所有智能体并为每个智能体生成一个随机目标位置
        for (size_t i = 0; i < sim->getNumAgents(); ++i)
        {
            float x = ux(e);  // 随机生成x坐标
            float y = uy(e);  // 随机生成y坐标
            float z = uz(e);  // 随机生成z坐标
            int rand = ur(e); // 随机生成一个整数

            goals[i] = Vector3(x, y, z); // 设置智能体的目标位置

            std::cout << "random once successfully" << std::endl; // 输出成功信息
        }
    }

    // 用于为智能体生成新的目标位置，目标位置的生成模式可以是“默认”模式或“随机”模式。
    void RVOPlanner::randGoal(const float limit_goal[6], const std::string &model)
    {
        // 提取目标位置的范围
        float x_min = limit_goal[0];
        float x_max = limit_goal[1];
        float y_min = limit_goal[2];
        float y_max = limit_goal[3];
        float z_min = limit_goal[4];
        float z_max = limit_goal[5];

        // 初始化随机数生成器
        std::random_device rd;                                  // 用于生成随机种子
        std::default_random_engine e(rd());                     // 使用随机种子初始化随机数引擎
        std::uniform_real_distribution<float> ux(x_min, x_max); // 均匀分布生成x坐标
        std::uniform_real_distribution<float> uy(y_min, y_max); // 均匀分布生成y坐标
        std::uniform_real_distribution<float> uz(z_min, z_max); // 均匀分布生成z坐标
        std::uniform_int_distribution<int> ur(0, 10);           // 均匀分布生成0到10之间的整数

        // 遍历所有智能体并为每个智能体生成一个随机目标位置
        for (size_t i = 0; i < sim->getNumAgents(); ++i)
        {
            float x = ux(e);  // 随机生成x坐标
            float y = uy(e);  // 随机生成y坐标
            float z = uz(e);  // 随机生成z坐标
            int rand = ur(e); // 随机生成一个0到10之间的整数

            // 如果是初始目标设置
            if (!IfInitial)
            {
                goals.emplace_back(x, y, z); // 添加新的目标位置到goals列表中
            }
            else if (model == "default") // 如果模式是"default"
            {
                // 如果当前目标与智能体位置之间的距离平方小于阈值
                if (absSq(goals[i] - sim->getAgentPosition(i)) < goal_threshold)
                {
                    goals[i] = Vector3(x, y, z); // 更新目标位置为新的随机位置
                }
            }
            else if (model == "random") // 如果模式是"random"
            {
                // 如果随机生成的整数大于8
                if (rand > 8)
                {
                    goals[i] = Vector3(x, y, z); // 更新目标位置为新的随机位置
                }
            }
        }
    }

    // 检测是否到达
    bool RVOPlanner::arrived()
    {
        bool reach = true; // 初始化标志变量，假设所有智能体都已经到达目标位置

        // 遍历所有智能体
        for (size_t i = 0; i < sim->getNumAgents(); ++i)
        {
            // 计算智能体当前位置与目标位置之间的距离平方
            if (absSq(goals[i] - sim->getAgentPosition(i)) >= goal_threshold)
            {
                reach = false; // 如果有智能体未到达目标位置，则将标志变量设置为false
            }
        }

        return reach; // 返回标志变量
    }

    // 用于检查智能体目标位置（goals）和智能体列表（agents_）是否已经初始化。
    void RVOPlanner::setInitial()
    {
        // 检查goals和agents_是否非空，并根据结果设置IfInitial
        // IfInitial为true表示goals和agents_都已初始化，为false表示至少有一个未初始化
        IfInitial = (!goals.empty()) && (!sim->agents_.empty());
    }

    // 用于为仿真中的每个智能体设置偏好速度（Preferred Velocity）。
    void RVOPlanner::setPreferredVelocities()
    {
        // 遍历所有智能体
        for (size_t i = 0; i < sim->getNumAgents(); ++i)
        {
            // 获取智能体当前位置与目标位置之间的距离平方
            if (absSq(goals[i] - sim->getAgentPosition(i)) < goal_threshold)
            {
                // 如果智能体接近目标位置，将其偏好速度设置为零向量
                sim->setAgentPrefVelocity(i, RVO::Vector3(0.0f, 0.0f, 0.0f));
            }
            else
            {
                // 如果智能体未接近目标位置，将其偏好速度设置为指向目标位置的单位向量
                sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i)));
            }
        }
    }

   // 修改代码，使之可以应对不同的数量的uav输入
    void RVOPlanner::updateState_true(const UAVPosVel::ConstPtr& model_msg)
    {
        sim->agents_.clear();

        int num = model_msg->uav_count;
        int count = 0; // 计数器，用于跟踪智能体数量

        for (int i = 0; i < num; i++){
            // 获取智能体的位置和速度
            float obs_x = model_msg->uav_positions[i].position.x;
            float obs_y = model_msg->uav_positions[i].position.y;
            float obs_z = model_msg->uav_positions[i].position.z;
            float vel_x = model_msg->uav_velocities[i].linear.x;
            float vel_y = model_msg->uav_velocities[i].linear.y;
            float vel_z = model_msg->uav_velocities[i].linear.z;

            sim->addAgent(RVO::Vector3(obs_x, obs_y, obs_z));
            sim->agents_[count]->velocity_ = RVO::Vector3(vel_x, vel_y, vel_z);

            // // 如果智能体已经初始化，则更新其位置和速度
            // if (IfInitial)
            // {
            //     sim->agents_[count]->position_ = RVO::Vector3(obs_x, obs_y, obs_z);
            //     sim->agents_[count]->velocity_ = RVO::Vector3(vel_x, vel_y, vel_z);
            // }
            // // 如果智能体尚未初始化，则将其添加到仿真环境中
            // else
            // {
            //     sim->addAgent(RVO::Vector3(obs_x, obs_y, obs_z));//这里添加了
            // }

            count++; // 增加智能体计数器

        }

    }

    // 用于更新仿真环境中智能体的状态信息。该函数从Gazebo仿真器中获取智能体的位置和速度信息，并将这些信息更新到RVO仿真环境中。
    void RVOPlanner::updateState_gazebo(gazebo_msgs::ModelStates::ConstPtr model_msg, std::string agent_name, int uav_num)
    {
        // 检查仿真器是否为"gazebo"
        if (simulator == "gazebo")
        {
            sim->agents_.clear();

            auto models_name = model_msg->name; // 获取所有模型的名称
            int num = models_name.size();       // 获取模型数量
            int count = 0;                      // 计数器，用于跟踪智能体数量
            RVO::Vector3 base_uav_pos, base_uav_vel;

            for (int i = 0; i < num; i++){
                std::string full_agent_name = agent_name + std::to_string(i); // 构建完整的智能体名称

                // 查找当前智能体名称在模型名称列表中的位置
                auto iter_agent = std::find(models_name.begin(), models_name.end(), full_agent_name);
                int agent_index = iter_agent - models_name.begin(); // 获取智能体在模型名称列表中的索引

                if (iter_agent != models_name.end())
                {
                    float obs_x = model_msg->pose[agent_index].position.x;
                    float obs_y = model_msg->pose[agent_index].position.y;
                    float obs_z = model_msg->pose[agent_index].position.z;
                    float vel_x = model_msg->twist[agent_index].linear.x;
                    float vel_y = model_msg->twist[agent_index].linear.y;
                    float vel_z = model_msg->twist[agent_index].linear.z;

                    if(i == uav_num){
                    base_uav_pos = RVO::Vector3(obs_x, obs_y, obs_z);
                    base_uav_vel = RVO::Vector3(vel_x, vel_y, vel_z);
                    }
                }
                

            }


            // 遍历所有模型
            for (int i = 0; i < num; i++)
            {
                std::string full_agent_name = agent_name + std::to_string(i); // 构建完整的智能体名称

                // 查找当前智能体名称在模型名称列表中的位置
                auto iter_agent = std::find(models_name.begin(), models_name.end(), full_agent_name);
                int agent_index = iter_agent - models_name.begin(); // 获取智能体在模型名称列表中的索引
                
                


                // 如果找到了智能体
                if (iter_agent != models_name.end())
                {
                    float obs_x = model_msg->pose[agent_index].position.x;
                    float obs_y = model_msg->pose[agent_index].position.y;
                    float obs_z = model_msg->pose[agent_index].position.z;
                    float vel_x = model_msg->twist[agent_index].linear.x;
                    float vel_y = model_msg->twist[agent_index].linear.y;
                    float vel_z = model_msg->twist[agent_index].linear.z;

                    // 获取智能体的相对位置和速度
                    float relative_obs_x = model_msg->pose[agent_index].position.x -base_uav_pos[0];
                    float relative_obs_y = model_msg->pose[agent_index].position.y - base_uav_pos[1];
                    float relative_obs_z = model_msg->pose[agent_index].position.z - base_uav_pos[2];
                    float relative_vel_x = model_msg->twist[agent_index].linear.x - base_uav_vel[0];
                    float relative_vel_y = model_msg->twist[agent_index].linear.y - base_uav_vel[1];
                    float relative_vel_z = model_msg->twist[agent_index].linear.z - base_uav_vel[2];

                    // 绝对位置速度
                    sim->addAgent(RVO::Vector3(obs_x, obs_y, obs_z));
                    sim->agents_[count]->velocity_ = RVO::Vector3(vel_x, vel_y, vel_z);
                    
                    // 相对位置速度
                    // Vector3 relative_position_; // other->position_ - position_
		            // Vector3 relative_velocity_; // velocity_ - other->velocity_
                    // sim->addAgent(RVO::Vector3(obs_x, obs_y, obs_z), base_uav_pos);
                    // sim->agents_[count]->relative_velocity_ = - RVO::Vector3(relative_vel_x, relative_vel_y, relative_vel_z);


                    // // 如果智能体已经初始化，则更新其位置和速度
                    // if (IfInitial)
                    // {
                    //     sim->agents_[count]->position_ = RVO::Vector3(obs_x, obs_y, obs_z);
                    //     sim->agents_[count]->velocity_ = RVO::Vector3(vel_x, vel_y, vel_z);
                    // }
                    // // 如果智能体尚未初始化，则将其添加到仿真环境中
                    // else
                    // {
                    //     sim->addAgent(RVO::Vector3(obs_x, obs_y, obs_z));
                    // }

                    count++; // 增加智能体计数器
                }
            }
        }
        // 如果仿真器不是"gazebo"，输出错误信息
        else
        {
            std::cout << "error: please check the simulator" << std::endl;
        }
    }

    // 用于执行一次仿真步骤。该函数更新所有智能体的邻居信息，计算每个智能体的新速度，并返回包含新速度的向量列表。
    std::vector<RVO::Vector3 *> RVOPlanner::step()
    {
        // 构建K-D树，用于加速邻居查询
        sim->kdTree_->buildAgentTree();

        // 清空newVelocities向量，准备存储新的速度
        newVelocities.clear();

        // 遍历所有智能体
        for (auto &agent : sim->agents_)
        {
            // 计算每个智能体的邻居，是在neighborDist范围内找邻居
            agent->computeNeighbors();

            // 计算每个智能体的新速度，计算了速度结果newVelocity_
            agent->computeNewVelocity();

            // 创建一个新的速度向量，并存储在newVelocities向量中
            auto *new_vel = new RVO::Vector3(agent->newVelocity_.x(), agent->newVelocity_.y(), agent->newVelocity_.z());
            newVelocities.push_back(new_vel);
        }

        // 返回包含新速度的向量列表，只需要第一个
        return newVelocities;
    }

} // namespace RVO
